import numpy as np


class camera():
    def __init__(self):
        # self.camera=np.asarray([[0.1, 0, 0],  # 顶点1
        #                                     [0, 0.1, 0],  # 顶点2
        #                                     [0, 0, 0],  # 顶点4
        #                                     [0.1, 0, 0],  # 顶点1
        #                                     [0, 0, 0.1],  # 3
        #                                     [0, 0, 0],  # 顶点4
        #                                     [0, 0.1, 0],  # 顶点2
        #                                     [0, 0, 0.1]], dtype="float32")  # 3
        self.camera_center=np.asarray([0, 0, 0], dtype="float32")
        self.matrix_x=[0.1, 0, 0]
        self.matrix_y = [0, 0.1, 0]
        self.matrix_z = [0, 0, 0.1]
        self.camera = np.asarray([self.matrix_x,  # 顶点1
                                             self.matrix_y,  # 顶点2
                                             self.camera_center,  # 顶点4
                                             self.matrix_x,  # 顶点1
                                             self.matrix_z,  # 3
                                             self.camera_center,  # 顶点4
                                             self.matrix_y,  # 顶点2
                                             self.matrix_z]  # 3
                                            , dtype="float32")
        self.matrix = np.asarray(
            [self.matrix_x,self.matrix_y, self.matrix_z])
    def update(self):
        self.camera_draw_point = np.asarray([self.matrix_x,  # 顶点1
                                             self.matrix_y,  # 顶点2
                                             self.camera_center,  # 顶点4
                                             self.matrix_x,  # 顶点1
                                             self.matrix_z,  # 3
                                             self.camera_center,  # 顶点4
                                             self.matrix_y,  # 顶点2
                                             self.matrix_z]  # 3
                                            , dtype="float32")

    def set_camera(self):

        return  self.camera
    def update_camera_position(self,center,size=0.1):
        self.camera_center = center
        self.camera[2]=self.camera_center
        self.camera[5] = self.camera_center
        self.matrix_x = [size, 0, 0]
        self.matrix_y = [0, size, 0]
        self.matrix_z = [0, 0, size]
        self.camera = np.asarray([self.matrix_x,  # 顶点1
                                  self.matrix_y,  # 顶点2
                                  self.camera_center,  # 顶点4
                                  self.matrix_x,  # 顶点1
                                  self.matrix_z,  # 3
                                  self.camera_center,  # 顶点4
                                  self.matrix_y,  # 顶点2
                                  self.matrix_z]  # 3
                                 , dtype="float32")
        return self.camera

    def update_camera_pose(self, rotation_matrix):
        self.camera=np.matmul(self.camera, rotation_matrix)
        return self.camera
